/*
 * Copyright (C) 2016 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdint.h>
#include <string.h>
#include <atomicBitset.h>
#include <atomicBitset_se.h>
#include <atomic_se.h>
#include <cpu.h>


void atomicBitsetInit(struct AtomicBitset *set, uint32_t numBits)
{
    set->numBits = numBits;
    memset(set->words, 0, sizeof(uint32_t) * ATOMIC_BITSET_NUM_WORDS(numBits));

    if (numBits & 31) //mark all high bits so that atomicBitsetFindClearAndSet() is simpler
        set->words[numBits / 32] = ((uint32_t)((int32_t) - 1LL)) << (numBits & 31);
}

uint32_t atomicBitsetGetNumBits(const struct AtomicBitset *set)
{
    return set->numBits;
}

bool atomicBitsetGetBit(const struct AtomicBitset *set, uint32_t num)
{
    if (num >= set->numBits) /* any value is as good as the next */
        return false;

    return !!((set->words[num / 32]) & (1UL << (num & 31)));
}

void atomicBitsetClearBit(struct AtomicBitset *set, uint32_t num)
{
    uint32_t idx = num / 32, mask = 1UL << (num & 31);
    uint32_t *wordPtr = set->words + idx;
    uint64_t states = 0;

    if (num >= set->numBits)
        return;

    states = cpuIntsOff();
    *wordPtr &= mask;
    cpuIntsRestore(states);
}

void atomicBitsetSetBit(struct AtomicBitset *set, uint32_t num)
{
    uint32_t idx = num / 32, mask = 1UL << (num & 31);
    uint32_t *wordPtr = set->words + idx;
    uint64_t states = 0;

    if (num >= set->numBits)
        return;

    states = cpuIntsOff();
    *wordPtr |= mask;
    cpuIntsRestore(states);
}
static int fls(int x)
{
    int r = 32;

    if (!x)
        return 0;

    if (!(x & 0xffff0000u)) {
        x <<= 16;
        r -= 16;
    }

    if (!(x & 0xff000000u)) {
        x <<= 8;
        r -= 8;
    }

    if (!(x & 0xf0000000u)) {
        x <<= 4;
        r -= 4;
    }

    if (!(x & 0xc0000000u)) {
        x <<= 2;
        r -= 2;
    }

    if (!(x & 0x80000000u)) {
        x <<= 1;
        r -= 1;
    }

    return r;
}

int32_t atomicBitsetFindClearAndSet(struct AtomicBitset *set)
{
    uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(set->numBits);
    uint32_t scratch = 0, bit = 0;
    uint32_t *wordPtr = set->words;
    int32_t final_bit = -1;

    uint64_t states = cpuIntsOff();

    for (idx = 0; idx < numWords; idx++, wordPtr++) {
        scratch = *wordPtr;
        bit = fls(~scratch) - 1;

        if (bit != 32 && bit != -1) {
            *wordPtr = scratch | (1 << bit);
            final_bit = (idx * 32) + bit;
            break;
        }
    }

    cpuIntsRestore(states);
    return final_bit;
}

bool atomicBitsetXchg(struct AtomicBitset *atomicallyAccessedSet, struct AtomicBitset *otherSet)
{
    uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(atomicallyAccessedSet->numBits);

    if (atomicallyAccessedSet->numBits != otherSet->numBits)
        return false;

    for (idx = 0; idx < numWords; idx++)
        otherSet->words[idx] = atomicXchg32bits(&atomicallyAccessedSet->words[idx], otherSet->words[idx]);

    return true;
}

bool atomicBitsetBulkRead(struct AtomicBitset *set, uint32_t *dest, uint32_t numBits)
{
    uint32_t idx, numWords = ATOMIC_BITSET_NUM_WORDS(set->numBits);

    if (set->numBits != numBits)
        return false;

    for (idx = 0; idx < numWords; idx++)
        dest[idx] = atomicRead32bits(&set->words[idx]);

    return true;
}
